
MEC4127F: Introduction to Robotics
Chapter 3: Describing pose
3.1 Introduction
This chapter will largely focus on the interrelation between angular velocity and orientation of different representations. The latter part of this chapter will then develop an exact mapping that can be used to relate pose-rate information (translational and rotational velocity) to pose.
Based on our results in Chapter 2, we now have a means of encoding the orientation of our robot body with respect to a reference frame of interest. The rotation matrix,
, is a useful starting point in containing orientation and also plays a pivotal role in mapping vectors between frames. However, our prior knowledge of mathematics means that we are probably more comfortable with visualising rotations using individual angles. In fact, while the rotation matrix requires 9 elements to be fully populated based on the direction cosine formulation of we actually only need to know as little as 3 distinct values to define the orientation. This follows from the fact that an orthogonal matrix imposes 6 constraints on the 9-element matrix
; 3 from the unit-norm requirement of and 3 from the orthogonality condition of
With 9 unknown terms in
and 6 constraints, this results in 3 free parameters. This result should not be terribly surprising considering that we are working in a three-dimensional space with three axes. Note that in the special case of planar rotation, as discussed in Section 2.3.3, the orientation corresponding to the rotation matrix
is uniquely described by the single angle θ.
3.1.1 Representing orientation
There are a number of different ways to represent orientation. While rotation matrices have their uses in performing vector or frame mappings, it is not always obvious what orientation is being described within the
matrix form. Additionally, when visualising orientation on a time-domain plot, for example to assess the attitude of a spacecraft, vector descriptions of orientation make more sense. We will also see later in this course that vector-based orientation descriptors fit better in state estimation and control design techniques. We will consider four main methods of representing orientation (directly or indirectly) in this course:
- Rotation matrices
- Euler angles
- Quaternions
- Exponential coordinates
Notably, each of these approaches has relative strengths and weaknesses. We will see later in this Chapter that Euler angles and exponential coordinates are a useful means of directly describing orientation, whereas quaternions and rotation matrices indirectly describe orientation and are ways to encode orientation in a functional way to facilitate vector or frame mappings.
3.2 Euler angles
Euler angles are arguably the most common representation used when describing orientation in robotics and can be used to encode/decode orientation information from the rotation matrix. In the case of capturing the frame
orientation with respect to
, the Euler angle vector is given by which uniquely describes the relative orientation using three angles:
- roll (ϕ),
- pitch (θ),
- yaw (ψ).
These three angles have the following bounds:
,
.
Euler angles describe a particular orientation using three disparate rotations. The basic concept is that we can construct the orientation of one reference frame relative to another by using three sequential, distinct rotations about the principal axes of one of the frames.
3.2.1 Rotation sequence
In the case of the single rigid-body problem, and using our two frames of interest,
and
, we are going to make three sequential rotations about our current reference frame, moving "backwards" from frame
, eventually to frame
. We begin in frame
with a null rotation matrix of
(our current reference frame that is attached to the robot body and
are aligned). With reference to Figure 3.1, we start by first rotating about the current z -axis,
, with angle ψ (shown in the leftmost image). Figure 3.1: Intrinsic Euler ZYX rotation sequence.
The rotation matrix that corresponds to this rotation about
, rotating from
to the new frame
, is
is known as an elementary rotation and notably only affects vectors in the
-
plane (the z component is unaffected). Note that we have implicitly introduced a new reference frame,
, from this rotation, with the above equation describing the orientation of
relative to
. Following this, we then rotate about the y-axis of
,
, with angle θ. The incremental rotation matrix that rotates
to
isWe have again introduced a new reference frame,
, using an elementary rotation. The final step is to rotate about
with angle ϕ. The corresponding incremental rotational mapping from
to
(our body-frame of interest) is We can now stitch together the sequential rotations (keeping an eye on the subscripts and superscripts to aid the multiplication order) to describe our body-frame orientation, relative to the world frame:
This representation makes use of Z-Y-X Euler angles, with the"Z-Y-X" term indicating the rotation order. The rotation sequence that we have just followed can be thought of as so-called intrinsic, as the three rotations are performed around one of the "new" reference frame principal axes. Extrinsic Euler angles by contrast result from performing three rotations about different combinations of the world-frame axes.
The form derived above is commonly referred to as Tait-Bryan Angles, or nautical angles. That is, we first rotated about
, followed by a rotation about
, and finishing with a rotation about
. Explicitly, our rotation matrix, parameterised by the Z-Y-X Euler angles, is written as and after performing the matrix multiplication yields
Rz = [cos(psi) -sin(psi) 0;
Ry = [cos(alpha) 0 sin(alpha);
-sin(alpha) 0 cos(alpha)];
R = Rz*Ry*Rx
R =

While the matrix above may seem a bit overwhelming, it is important to note that we only require three variables to fully define it, namely the Euler angles -
, which respectively describe roll, pitch, and yaw — see Figure 3.2 for a useful mnemonic to remember the rotation axes. Figure 3.2: Mnemonics to remember the roll, pitch and yaw angle rotation axes [5].
The code block below can be used to visualise the workings of Euler angles in the four different frames that show the progression from
to
, namely 
phi = 0*pi/180; %define roll angle about x axis of {Y}
the = -30*pi/180; %define pitch angle about y axis of {Z}
psi = -10*pi/180; %define yaw angle about z axis of {W}
Rz = [cos(psi) -sin(psi) 0; %principal z-axis rotation matrix
Ry = [cos(the) 0 sin(the); %principal y-axis rotation matrix
Rx = [1 0 0; %principal x-axis rotation matrix
subplot(2,2,1),plotTransforms([0 0 0],rotm2quat(eye(3)),"MeshFilePath",'fixedwing.stl',"MeshColor","red","FrameAxisLabels","on","FrameLabel","W")
axis equal, title('$^W{\bf R}_W={\bf I}$',Interpreter='latex'),grid on
subplot(2,2,2),plotTransforms([0 0 0],rotm2quat(Rz),"MeshFilePath",'fixedwing.stl',"MeshColor","green","FrameAxisLabels","on","FrameLabel","Z")
axis equal, title('$^W{\bf R}_Z$',Interpreter='latex'),grid on
subplot(2,2,3),plotTransforms([0 0 0],rotm2quat(Rz*Ry),"MeshFilePath",'fixedwing.stl',"MeshColor","blue","FrameAxisLabels","on","FrameLabel","Y")
axis equal, title('$^W{\bf R}_Y={^W{\bf R}_Z}{^Z{\bf R}_Y}$',Interpreter='latex'),grid on
subplot(2,2,4),plotTransforms([0 0 0],rotm2quat(Rz*Ry*Rx),"MeshFilePath",'fixedwing.stl',"MeshColor","black","FrameAxisLabels","on","FrameLabel","B")
axis equal, title('$^W{\bf R}_B={^W{\bf R}_Z}{^Z{\bf R}_Y}{^Y{\bf R}_B}$',Interpreter='latex'),grid on
sgtitle('Visualisation of sequential Euler angle rotations')
While the Z-Y-X rotation order is fairly common in engineering (especially aeronautics and aerial robotics), we could have actually chosen from a multitude of rotation orders. Our only constraint is that we cannot rotate about the same axis sequentially, as that would be equivalent to a single rotation by the sum of the two angles. The twelve valid rotation orders are:
3.2.2 Gimbal lock
Different industries and manufacturers will use different rotation conventions, selecting one from above, but it is important to note there is technically no right or wrong version. However, one crucial point to be aware of is that an Euler angle representation of orientation always exhibits a singularity when any two rotation axes become parallel or anti-parallel. This phenomenon is commonly referred to as gimbal lock.
Definition: Gimbal lock
Gimbal lock is the loss of one degree of freedom in a three-dimensional, three-gimbal mechanism that occurs when the axes of two of the three gimbals are driven into a parallel configuration, "locking" the system into rotation in a degenerate two-dimensional space.
For the set of Euler angle rotation orders with repeated axes (sometimes referred to as Eulerian), namely
gimbal lock will occur when the second angle of rotation equals
, where
, as this will make the first and third rotation axis align. For the set of Euler angles with no repeated axes (sometimes referred to as Cardian), namely
gimbal lock will occur when the second angle of rotation equals
, as this will make the first and third rotation axis align. In summary, regardless of the Euler rotation convention, gimbal lock will occur when the first and third rotation axes are aligned. Physically, this means that rotations about the first and third axis cannot be differentiated apart, and are instead summed, as a result of degenerate 2D space. Figure 3.3 below gives a visualisation of gimbal lock. Figure 3.3: Visualisation of the three rotational axes (facilitated by imaginary gimbals) attached to an airplane. Left: nominal condition with none of the axes aligned. Right: Two of the gimbals become aligned causing a loss of information in one of the axes and resulting in a singularity [6]
So if you plan to use Euler angles, you need to be cognizant of how the angular behaviour is going to evolve. If the operating region contains Euler angles that cause the first and last rotation axes to become parallel, then consider re-framing how you have defined your reference frame, or use a different rotation order to circumvent the potential for gimbal lock. This video provides a good explanation of gimbal lock and the singularity problem. 3.2.3 Extracting Euler angles
shows us how to populate a rotation matrix given our Z-Y-X ordering,
, but we can also extract Euler angle information from a rotation matrix if required: We may, for example, need Euler angle information to visualise how our robot is oriented, or we may want to make use of that information within a control loop (e.g. regulating the yaw angle of a manipulator arm). Denoting the row x and column y element of the rotation matrix
as
, namely the corresponding Euler angles can be extracted using
Importantly, this result is only valid for Z-Y-X intrinsic Euler angles.
phi = -40*pi/180; %set roll angle about x axis of {Y}
the = 90*pi/180; %set pitch angle about y axis {Z}
psi = -30*pi/180; %set yaw angle about z axis of {W}
Rz = [cos(psi) -sin(psi) 0; %principle rotation about z-axis
Ry = [cos(the) 0 sin(the); %principle rotation about y-axis
Rx = [1 0 0; %principle rotation about x-axis
R = Rz*Ry*Rx; %combining all three rotations into 3D rotation matrix
%extracting Euler angles using formula above.
phi_ = atan2( R(3,2),R(3,3) )*180/pi
theta_ = -asin( R(3,1) )*180/pi
psi_ = atan2( R(2,1),R(1,1) )*180/pi
Note that in practice we make use of atan2 instead of atan when performing the calculations to determine ϕ and ψ, as this takes into account the full possible range of
. 3.2.4 Intrinsic and extrinsic rotations
Recall our Euler angle result of
which we assembled from left to right using the ordering of
Notably, we can also think of constructing the result above, building instead from right to left, by starting in
and then: - performing a rotation of ϕ about
, - followed by a rotation of θ about
, - and then finally, a rotation of ψ about
.
This gives us a clue about intrinsic and extrinsic rotations:
- when a frame is rotated about a vector in
, we should post-multiply the current orientation with the matrix describing the new rotation, - when a frame is rotated about a vector in
, we should pre-multiply the current orientation with the matrix describing the new rotation.
Example: Intrinsic and extrinsic rotations
In this example the robot has a initial orientation described by
If the robot then experiences a rotation of ψ about
, we can describe the resulting orientation as which is obtained by post-multiplying
by
. This constitutes an intrinsic rotation. If instead, the robot with initial orientation of
experiences a rotation of ψ about
, we can describe the resulting orientation as which is obtained by pre-multiplying
by
. This constitutes an extrinsic rotation. The code block below provides a visual representation of this.
Rz = [cos(psi) -sin(psi) 0; %principle rotation about z-axis
R = [1 0 0; %initial orientation of robot
R1 = R*Rz; %new orientation after intrinsic rotation about z axis
R2 = Rz*R; %new orientation after extrinsic rotation about z axis
subplot(1,3,1),plotTransforms([0 0 0],rotm2quat(R),"MeshFilePath",'fixedwing.stl',"MeshColor","red","FrameAxisLabels","on","FrameLabel","W")
axis equal, title('${\bf R}$',Interpreter='latex'),grid on
subplot(1,3,2),plotTransforms([0 0 0],rotm2quat(R1),"MeshFilePath",'fixedwing.stl',"MeshColor","green","FrameAxisLabels","on","FrameLabel","Z")
axis equal, title('${\bf R}{\bf R}_z$',Interpreter='latex'),grid on
subplot(1,3,3),plotTransforms([0 0 0],rotm2quat(R2),"MeshFilePath",'fixedwing.stl',"MeshColor","blue","FrameAxisLabels","on","FrameLabel","Z")
axis equal, title('${\bf R}_z{\bf R}$',Interpreter='latex'),grid on
sgtitle('Comparison of intrinsic and extrinsic rotations')
3.3 Quaternions
Euler angles are appealing as an introduction to understanding orientation, as they are intuitively pleasing and relatively easy to visualise. However, the potential for our orientation representation becoming degenerate and resulting in kinematic singularities raises a red flag for many robotic applications. Additionally, the idea of describing an orientation using three sequential rotations does not follow how rigid-body robot motion will evolve in general. Instead, we would expect pure rotations about some arbitrary axis — enter the quaternion!
The quaternion is a popular choice of encoding attitude that can completely avoid the issue of gimbal lock by rather describing the orientation as a single rotation about some defined axis that intercepts the relevant reference frame origin. At first glance, the quaternion can seem a bit daunting, but the benefits of understanding and being able to utilise it in robotics (among other fields) is worth the time investment that you may need to understand it. We will also see later in Section 3.4 that quaternions are closely related to exponential coordinates — another powerful and yet intuitive way to describe orientation. 3.3.1 Definition
The quaternion,
, is a 4-element vector (also known as a 4-tuple) and is structured as in which
are real numbers. In the context of describing orientation, we only consider unit quaternions, which gives us the unit-norm constraint of Explicitly, the unit quaternion follows an angle-axis structure of
where
is the unit-length (
) rotation axis, and
is the rotation angle, as shown in Figure 3.4 for the case when the rotation vector is described with respect to
. Figure 3.4: Visualisation of a quaternion-based rotation about vector
with rotational angle α. The code block below also shows how the quaternion axis-angle form can be used to rotate a rigid body, such as a quadcopter. The black line shows the rotation axis,
. n = n/norm(n); %normalise v so that it always has unit length.
q = [cos(alpha/2); sin(alpha/2)*n];
plotTransforms([0 0 0],[1 0 0 0],"MeshFilePath",'multirotor.stl',"MeshColor","red")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('Before rotation')
plotTransforms([0 0 0],q',"MeshFilePath",'multirotor.stl',"MeshColor","blue")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('After rotation')
sgtitle('Comparison of quadcopter before and after experiencing a specified rotation')
The explicit quaternion form of
is a way of encoding the 3D rotation of α about
in a useful format, analogous to how rotation matrices encode attitude. Assuming we are given
, we can always extract the angle-axis information from our quaternion representation using and
When
, (which implies that
), the equation above tends to infinity, making the rotation axis become undefined. However, this is not problematic, as
corresponds to the null rotation matrix of
. In essence, we cannot determine the rotation vector because there is no rotation taking place. In this case, we would describe the identity (or null) quaternion as
. Given an angle-axis combination, we can now describe the corresponding quaternion and similarly, given a quaternion, we can recover our angle-axis pair
For example, if we wanted to rotate about the
-axis by an angle
, our rotation axis would be
, resulting in Note that this quaternion is analogous to the principle z-axis rotation matrix of
The method of encoding the orientation is just different. Fortunately, there does exist a one-to-one mapping between the quaternion representation and rotation matrix, given by
and unsurprisingly, by substituting our result for
into
above, we obtain
. For
, which implies that
, extraction of the quaternion from a rotation matrix is achieved using where
.If
, implying that that
, the equation above for determining the vector part of
will result in a singularity. In this case, we can instead determine
using 3.3.2 Quaternion multiplication
While the mapping from
to
is a nice sanity check on things, we are losing one of the benefits of the quaternion, and that is its mathematical efficiency. Just as we were able to sequentially construct our body-frame orientation by using the elementary rotations when dealing with Euler angles, we can follow a similar process for the quaternion. However, we first need to cover how quaternion multiplication works. Given two quaternions,
and
, the quaternion product (or Hamilton Product), indicated using the symbol ⊗, is defined as This stems from the fact that
and
can be written as
,respectively, where
are the basis vectors indicating direction. Using the relationship of yields the above-mentioned result of
The result above can be written compactly as
where
and
. Continuing with our exercise of emulating the sequential construction of our body-frame orientation as was done using ZYX Euler angles, we begin by configuring the two reference frames such that they are aligned and coincident. We then apply a rotation about
, with an angle of ψ, which gives us We next rotate about
with an angle of θ, which is encoded by the incremental quaternion of Lastly, we rotate about
, with an angle of ϕ, which is described by the incremental quaternion of Our full description of the body-frame orientation, using the quaternion as our encoder follows as
and is analogous to the rotation matrix equivalent of
The code block below shows how to perform quaternion multiplication in MATLAB and also illustrates that the quaternion representation of attitude matches that of the rotation matrix counterpart. phi = 90*pi/180; %set roll angle about x axis of {Y}
the = 30*pi/180; %set pitch angle about y axis of {Z}
psi = 0*pi/180; %set yaw angle about z axis of {W}
q_z2w = [cos(psi/2) 0 0 sin(psi/2)]; %quaternion representation of yaw rotation about z in {W}
q_y2z = [cos(the/2) 0 sin(the/2) 0]; %quaternion representation of pitch rotation about y in {Z}
q_b2y = [cos(phi/2) sin(phi/2) 0 0]; %quaternion representation of roll rotation about x in {Y}
q_b2w = quatmultiply( quatmultiply(q_z2w,q_y2z), q_b2y ) %full quaternion describing orientation of {B} wrt {W}
0.6830 0.6830 0.1830 -0.1830
figure,hold on,sgtitle('Representing Euler angle sequence using quaternions')
subplot(2,2,1),plotTransforms([0 0 0],[1 0 0 0],"MeshFilePath",'fixedwing.stl',"MeshColor","red","FrameAxisLabels","on","FrameLabel","W")
axis equal, title('$^W{\bf q}_W=[1~0~0~0]^T$',Interpreter='latex'),grid on
subplot(2,2,2),plotTransforms([0 0 0],q_z2w,"MeshFilePath",'fixedwing.stl',"MeshColor","green","FrameAxisLabels","on","FrameLabel","Z")
axis equal, title('$^W{\bf q}_Z$',Interpreter='latex'),grid on
subplot(2,2,3),plotTransforms([0 0 0],quatmultiply(q_z2w,q_y2z),"MeshFilePath",'fixedwing.stl',"MeshColor","blue","FrameAxisLabels","on","FrameLabel","Y")
axis equal, title('$^W{\bf q}_Y={^W{\bf q}_Z}\otimes {^Z{\bf q}_Y}$',Interpreter='latex'),grid on
subplot(2,2,4),plotTransforms([0 0 0],quatmultiply( quatmultiply(q_z2w,q_y2z),q_b2y),"MeshFilePath",'fixedwing.stl',"MeshColor","black","FrameAxisLabels","on","FrameLabel","B")
axis equal, title('$^W{\bf q}_B={^W{\bf q}_Z}\otimes {^Z{\bf q}_Y}\otimes {^Y{\bf q}_B}$',Interpreter='latex'),grid on
As with the Euler angles, we have followed the ordering of
Quaternions can also be thought of as being intrinsic or extrinsic — when post-multiplying by a quaternion, we can imagine the rotation vector being described in the current frame (as above), whereas when pre-multiply the vector is described in
. Note that in this particular case, our quaternions are still parameterised by the Euler angles. This is not how we will usually make use of quaternions; rather, this is a demonstration of how quaternion rotation works, and its relationship with Euler angles and rotation matrices.
It is worth highlighting the numerical efficiency of the quaternion — while multiplying two rotation matrices requires 27 multiplications and 18 additions, quaternion multiplication only requires 16 multiplications and 12 additions. This may seem insignificant, but in certain applications where processing power is limited, or a large number of coordinate mappings are required (e.g. in the video game industry) this FLOP saving is invaluable!
As with the rotation matrix, the quaternion has an inverse, defined as
We can think of this inverse in one of two ways: either
- (i) the rotation vector changes direction and points in the opposite direction, or
- (ii) the rotation angle changes direction.
In either case, the quaternion inverse can be thought of as "undoing" a rotation.
If we rotated our robot such that our quaternion was
, followed by multiplication of the inverse,
, we would return to our identity quaternion,
. alpha = 45*pi/180; %set rotation angle
nx = [1 1 1]; %define arbitrary vector
nx = nx/norm(nx); %set vector norm to one
q = [cos(alpha/2) sin(alpha/2)*nx]; %quaternion describing rotation
q_I = quatmultiply( quatinv(q),q ) %quaternion multiplied by its inverse, which always returns the null quaternion
3.3.3 Rotating a frame
The sequential process of building a quaternion using the principal axis rotations is useful for demonstrating the parallel between rotation matrices and quaternions, but there is actually no need to ever use Euler angles when incorporating quaternions. As previously mentioned, the orientation can be defined using a rotation vector,
, and rotation angle α about
. In other words, we can define a mapping between two reference frames by either rotating about a single vector with a specific rotation angle (encoded as a quaternion), or equivalently performing three sequential rotations about the intrinsic axes (Euler-parameterised rotation matrix). The quaternion approach is generally quite appealing for robotics, as it matches the "natural" rotational behaviour of rigid bodies. That is, incremental rotations will occur as a single motion, not three discrete motions. Later Chapters will also demonstrate that quaternions are highly beneficial when confronting the rigid-body robotic trajectory generation, orientation estimation, and control problem, whereas Euler angles can become problematic in most cases.
The code block below shows how rotation about a vector can be visualised, with the resulting pose after the rotation facilitated using quaternions.
n_x = 0; %x component of rotation vector
n_y = 0; %y component of rotation vector
n_z = 1; %z component of rotation vector
n = [n_x n_y n_z]'; %full rotation vector
n = n/norm(n); %normalise vector so that it always has unit length
alpha = 60*pi/180; %rotation angle
q = [cos(alpha/2); sin(alpha/2)*n]; %quaternion, formed from combination of rotation angle and rotation vector
plotTransforms([0 0 0],[1 0 0 0],"MeshFilePath",'fixedwing.stl',"MeshColor","red")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('Before rotation'),grid on
plotTransforms([0 0 0],q',"MeshFilePath",'fixedwing.stl',"MeshColor","blue")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('After rotation'),grid on
sgtitle('Visualising a rotated reference frame attached to a fixed-wing UAV')
3.3.4 Double cover
There is one minor caveat that should be made clear when using quaternions. We are operating in a 3D Euclidean space, but the quaternion is a 4-element vector. So why does the quaternion have 4 parameters to specify a 3D orientation? The simple answer is that a single quaternion does not uniquely define an orientation. That is, the orientation derived using a rotation about some axis is equivalent to a negative rotation about that same axis after it has been inverted. This results in a double cover phenomenon where
and
represent the same rotation. The code block demonstrates this result in general, showing that orientation
and
are equivalent. q0 = 1; %set scalar component of quaternion
qx = 0.2; %set x component of quaternion
qy = 0.7; %set y component of quaternion
qz = 0.4; %set z component of quaternion
q = [q0 qx qy qz]; %unnormalised quaternion
q = q/norm(q); %normalised quaternion
subplot(1,2,1),plotTransforms([0 0 0],q,"MeshFilePath",'multirotor.stl',"MeshColor","red","FrameAxisLabels","on","FrameLabel","W")
axis equal, title(['Rotation described by ','$\bf q$'],Interpreter='latex'),grid on
subplot(1,2,2),plotTransforms([0 0 0],-q,"MeshFilePath",'multirotor.stl',"MeshColor","green","FrameAxisLabels","on","FrameLabel","Z")
axis equal, title(['Rotation described by ','$\bf -q$'],Interpreter='latex'),grid on
sgtitle('Illustration of double cover')
Example: Double cover
You rotate an arbitrary reference frame about a specified rotation vector,
, by some angle ϕ, giving us the quaternion
. If we had decided to instead rotate about the same rotation vector but in the opposite direction with a rotation angle of
, our quaternion would follow as Noting the periodic nature of the cos and sin functions, we can make use of
and
to reduce
to
.As a result of this double covering, every orientation will map to two quaternions; one that describes the minimum angle rotation,
, and the other describing the maximum angle rotation,
. The only exception is the special case where the rotation angle is
, making both rotations equivalent in terms of angular distance. This disparity between a minimum and maximum angle rotation becomes important when our robot is trying to follow a path or correct an angular error, as we commonly desire minimal angular changes that result in smaller time and energy requirements (the minimum angle rotation). This is often referred to as the geodesic. 3.3.5 Rotating vectors
We have already shown that quaternions can be used to
- represent orientation and
- rotate frames.
Analogous to rotation matrices, quaternions also possess the third property of being able to map vectors between frames. We will not explore this in detail in this course, but the following formulation can be used to rotate vector
between two frames, such as from
to
: Note that this is equivalent to the rotation matrix form of
albeit less convenient to express mathematically when chaining together multiple sequential rotations.
pb = [1 2 3]'; %p wrt {B}
q = [1/sqrt(2) 0 1/sqrt(2) 0]; %quaternion describing orientation of {B} wrt {W}
pw = R*pb %p wrt {W} using rotation matrix approach
pb_q = [0 pb']; %"quaternionised" p wrt {B}
pw_q = quatmultiply( quatmultiply(q, pb_q),quatinv(q) ); %calculate "quaternionised" p wrt {W}
pw_ = pw_q(2:4)' %p wrt {W} using quaternion approach
3.4 Exponential coordinates of rotations
3.4.1 Definition
Directly related to quaternions, exponential coordinates parameterise an orientation in terms of a rotation axis, represented by a unit vector
, and an angle of rotation α; the vector
then serves as the three-parameter exponential coordinate representation of the rotation. Writing
and α individually is known as the axis-angle representation of a rotation and can be recovered from exponential coordinates of
using Note that the rotation angle in this case will always be positive.
Exponential coordinates can be used to describe orientation and can also be encoded in rotation matrices or quaternions to map vectors between frames. However, before we can show this result, we need to introduce the skew-symmetric matrix.
Definition: Skew-symmetric matrix
A skew-symmetric matrix is a square matrix whose transpose equals its negative:
. By this definition, the diagonal elements of
must all equal zero in order to satisfy
. Skew-symmetric matrices are useful in reposing a vector cross product operation as a matrix-vector multiplication, as we will show in a latter section. Given an arbitrary vector
, the corresponding skew-symmetric matrix is which shows the symmetry across the diagonal, albeit with a sign change. Note the use of
to indicate the skew-symmetric mapping. As we will explore later in detail, if we wanted to evaluate a cross product operation such as
, we could repose it as
, which is a vector multiplying into a matrix (not a cross product of two vectors). However, the result would remain the same. 3.4.2 Exponential coordinates of SO(3)
It can be shown that a rotation matrix, parameterised using exponential coordinates, is given by the matrix exponential mapping
Note that the matrix exponential differs from the standard single-element matrix exponential and in general is described by
where
is constant. For the special case when
is skew-symmetric, for example,
, the equation above can be reduced without loss of generality as The equation above is known as Rodrigues' formula for rotations and is a convenient means of building a rotation matrix from exponential coordinates without having to evaluate the matrix exponential (an operation that can be computationally expensive on embedded hardware).
Writing out the above equation explicitly yields
where
, using the shorthand notation of
and
. n_x = 1; %x component of rotation vector
n_y = 0.4; %y component of rotation vector
n_z = 0; %z component of rotation vector
n = n/norm(n); %normalised rotation vector
alpha = -70*pi/180; %rotation angle
S = [0 -n_z n_y; %skew-symmetric form of rotation vector
R = eye(3)+sin(alpha)*S+( 1-cos(alpha) )*S^2; %Rodrigues' formula to determine R
plotTransforms([0 0 0],[1 0 0 0],"MeshFilePath",'multirotor.stl',"MeshColor","red")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('Before rotation')
plotTransforms([0 0 0],rotm2quat(R),"MeshFilePath",'multirotor.stl',"MeshColor","blue")
line([0 n(1)],[0,n(2)],[0,n(3)],'Color','black','LineStyle','-','LineWidth',2)
axis equal,title('After rotation')
sgtitle('Visualising a rotated reference frame using exponential coordinates')
Note that when dealing with exponential coordinates in SO(2), the exponential coordinates collapses to
, where θ corresponds to the rotation experienced by one frame with respect to another whilst bound to a plane. Example: Using Rodrigues' formula
Determine the pose of
with respect to
when a frame initially aligned with
experiences a rotation about
with an angle of
rad. Using Rodrigues' formula, we obtain
n = [0 0.866 0.5]'; %rotation vector from text above
alpha = 30*pi/180; %rotation angle from text above
S = [0 -n(3) n(2); %skew-symmetric form of rotation vector
R = expm(S*alpha) %rotation matrix determined from matrix exponential
0.8660 -0.2500 0.4330
0.2500 0.9665 0.0580
-0.4330 0.0580 0.8995
R_ = eye(3)+sin(alpha)*S+( 1-cos(alpha) )*S^2 %rotation matrix determined using Rodrigues' formula
0.8660 -0.2500 0.4330
0.2500 0.9665 0.0580
-0.4330 0.0580 0.8995
3.4.3 Principal axis rotations
Recalling that our rotation matrix can be parameterised by the axis-angle representation as
we can easily form the three principal axis rotations of:
— rotation of α about x axis,
— rotation of α about y axis,
— rotation of α about z axis,
by adjusting the components of
in the rotation matrix equation above. Specifically, if we set
in the rotation matrix equation above, we obtain if we set
we obtain and if we set
we get
These results match of the Euler angle formulation supplied in Section 3.2.1. 3.4.4 Matrix logarithms of rotations
We can recover the exponential coordinates,
from a rotation matrix
using a matrix logarithm operation, which is the inverse of the matrix exponential. Specifically, if the rotation matrix is given by then the matrix logarithm of the rotation matrix yields
Based on this, we can think of the matrix exponential as in integration operator for the exponential coordinates, and the matrix logarithm as a differentiation operator.
Example: Matrix logarithm
Using MATLAB, determine the rotation matrix corresponding to
and
, and show that the exponential coordinates can be recovered using the matrix logarithm. n = [0 0.866 0.5]' %rotation vector
alpha = 30*pi/180 %rotation angle
S = [0 -n(3) n(2); %skew-symmetric form of rotation vector
R = expm(S*alpha); %rotation matrix from matrix exponential
S_ = logm(R); %matrix logarithm of rotation matrix
va = [-S_(2,3) S_(1,3) -S_(1,2)]'; %recovered exponential coordinates
alpha_ = norm( va ) %recovered rotation angle
v_ = va/alpha %recovered rotation vector
Instead of having to use the matrix logarithm, which is computationally heavy on embedded systems, an algorithm exists that can determine the exponential coordinates. The rotation angle can be determined from the rotation matrix using
where
corresponds to the trace of matrix
, namely, summing the diagonal entries of
:
.For
, the rotation vector (in skew-symmetric form) is then determined by For the special case of
, the rotation vector is one of the following solutions, which would need to be verified using
: 3.4.5 The dot product and cross product
Recall that the cross product of two unitary vectors is described by
where α describes the angle between vectors
and
, and
defines the vector perpendicular to
and
. Based on this description, if we were to rotate vector
about vector
with a rotation of α units, we would obtain
, namely As such,
can be thought of as exponential coordinates describing the mapping from
and
. This relationship is useful when dealing with multivariable control design (e.g. thrust vectoring control) and state estimation (comparing vector directions) — both of which will be explored later in this course. Note that when α is sufficiently small, the exponential coordinates can be suitably approximated by
. Recall the dot product of two unitary vectors:
Given known
and
, the rotation angle can be recovered by where
. The rotation vector is then determined by Note that the inverse cosine function will return a value of α between 0-π. Additionally, if
and
are anti-parallel, determination of
using the equation above will be incorrect, as
and
. Specifically, when vectors
and
are collinear (form a line), any rotation vector and rotation angle
will map
to
. In this case, one could arbitrarily choose a vector that lies in the plane orthogonal to this line. Example: Cross product and rotation matrix
a = [1 2 3]'; %arbitrary vector a
a = a/norm(a); %arbitrary normalised vector a
b = [-1 1 3]'; %arbitrary vector b
b = b/norm(b) %arbitrary normalised vector b
n = cross(a,b); %cross product of vectors a and b
alpha = acos( dot(a,b)); %rotation angle between a and b
c = n/sin(alpha); %rotation vector perpendicular to a and b
phi = alpha*c; %exponential coordinates
S = [0 -phi(3) phi(2); %skew-symmetric form of exponential coordinates
R = expm(S); %rotation matrix
b_ = R*a %determining vector b using rotation matrix
quiver3(0,0,0,a(1),a(2),a(3))
quiver3(0,0,0,b(1),b(2),b(3))
quiver3(0,0,0,c(1),c(2),c(3))
legend('$\hat{a}$','$\hat{b}$','$\hat{c}$','Interpreter','Latex')
sgtitle('Relationship between the cross product of two vectors and their relative orientation')
%set x-y axis limits and view angle
3.5 Angular velocity
Angular velocity will dictate how our orientation changes over time, so we are therefore interested in how the two are related when dealing with multiple reference frames. This is important when mathematically modelling systems (for example when designing simulation environments) and also when angular rate information is available from a sensor (for example a gyroscope) and we want to estimate orientation from it.
In the case of planar rotations (resulting in rotations about a fixed principle axis) the relationship is trivial, but we require more scrutiny for the generalised spatial case when up to three degrees of rotation freedom are at work.
3.5.1 Rotation matrix derivative
To understand the relationship between angular velocity and orientation, we need to develop a better understanding of reference frame motion as they rotate. Consider the motion of a point fixed to the end of
when
experiences and angular velocity of
, as shown in Figure 3.5. Figure 3.5: Reference frame
experiencing angular velocity.
experiences translational velocity based on angular velocity
. Recall that
describes the x-axis of the body frame when described in the world frame, which is obtained by Given the orthogonality property of
,
will always have unit length. This means that during rotation, only the direction of
will change — not the length. Given that the angular velocity of the body frame is described in the world frame as
, we can determine the rate of change of
based on elementary mechanics, namely
.We can repeat this process for the other two axes of
without loss of generality, which yields
.
.Recalling that
the derivative of the rotation matrix follows as
Making use of our skew-symmetric form for the cross product, namely
we can express the rotation matrix derivative as
This result is quite interesting, as it suggests that the rate of change of our orientation encoder will be a function of both the angular velocity vector and current orientation. It can be shown that for any
SO(3) and
, Recalling that we can relate the angular velocity between different frames using
and this, along with the result above of
can be used to determine the rotation matrix derivative as a function of
: Note the similarity when determining
as a function of
and
. 3.5.2 Quaternion derivative
The relationship between angular velocity and the quaternion follows a similar pattern to that of the rotation matrix counterpart. The full derivation will be spared here for sake of time. Analogous to the result of
, the quaternion derivative can be written as where
is a "pure" quaternion formed from the angular velocity vector in frame
. Using appropriate identities, the quaternion derivative can be written as where
which follows a similar structure but now using the angular velocity in
. 3.6 Obtaining orientation from angular velocity
If the angular velocity is known exactly, we can use this information to determine the corresponding orientation at a given time using integration. We must, however, take into account the particular reference frame mapping, such as from
to
when the velocity is described in
, before performing the integration operation — otherwise the resulting "orientation" described in
is meaningless in general. 3.6.1 Rotation matrix formulation
The incremental orientation can be determined in rotation matrix form using exponential coordinates. Recall that
where
are the exponential coordinates describing a generalised 3D rotation. Assuming a body experiences an angular velocity vector of
, expressed in
, for a duration of
units of time, we can determine the incremental exponential coordinates corresponding to rotating about vector
for
units of time (usually seconds) using The incremental rotation angle and normalised rotation vector follow respectively as
The corresponding incremental rotation matrix is given by
If our orientation is originally described as
prior to experiencing the angular velocity, we can then determine our updated orientation,
, by post-multiplying
by the incremental rotation matrix, namely where
is the incremental orientation as a result of the body-frame angular velocity. This process can be repeated any number of times, assuming one is able to capture each incremental rotation matrix that corresponds to the angular velocity that was experienced for the given time frame. This is generalised as Note that we are post-multiplying in each instance, as the rotation vector is described in
. If instead we had information of
, we would need to pre-multiply in each instance. 3.6.2 Quaternion formulation
Recall that the quaternion encodes the exponential coordinates. Given our previously defined incremental axis-angle representation from Section 3.6.1 of we can determine the corresponding incremental quaternion as
If our orientation is originally described by
prior to experiencing the angular velocity, we can then determine our updated orientation,
, by post-multiplying
by the incremental quaternion, namely where
is the incremental orientation as a result of the body-frame angular velocity. This process can be repeated any number of times, assuming one is able to capture each incremental rotation matrix that corresponds to the angular velocity that was experienced for the given time frame. This is expressed as As with the rotation matrix formulation, we are post-multiplying in each instance, as the rotation vector is described in
. If instead we had information of
, we would need to pre-multiply in each instance. Example: Determining quaternion-based orientation from angular velocity
The code block below provides a simulation of how angular velocity information can be correctly integrated and mapped to obtain a quaternion-based representation of orientation.
dT = 0.01; %sampling duration
numSamples = 1000; %number of samples
t = (0:numSamples-1)*dT; %time vector
omega1 = linspace(0,1,numSamples);
omega2 = linspace(0,0.2,numSamples);
omega3 = linspace(0,-0.5,numSamples);
omega = [omega1' omega2' omega3']; %generate arbitrary 3D angular velocity
q = zeros(numSamples,4); %initialise quaternion array
q(1,1:4) = [1 0 0 0]; %initialise first sample of quaternion to be null quaternion
dtheta = omega(k,:)*dT; %determine incremental exponential coordinates
dalpha = norm(dtheta); %determine incremental rotation angle
if( dalpha == 0) %determine incremental rotation vector
dq = [cos(dalpha) sin(dalpha)*n]; %populate incremental quaternion
q(k+1,1:4) = quatmultiply(q(k,1:4),dq); %calculate updated quaternion at sample instance k+1
q(k+1,1:4) = q(k+1,1:4)/norm( q(k+1,1:4) ); %normalise quaternion
%plot angular velocity figures
figure, sgtitle('Angular velocity behaviour over time')
plot(t,omega(:,1),lineWidth=2),xlabel('time (s)'),ylabel('\omega_x')
plot(t,omega(:,2),lineWidth=2),xlabel('time (s)'),ylabel('\omega_y')
plot(t,omega(:,3),lineWidth=2),xlabel('time (s)'),ylabel('\omega_z')
figure, sgtitle('Quaternion behaviour over time')
plot(t,q(:,1),lineWidth=2),xlabel('time (s)'),ylabel('q_0')
plot(t,q(:,2),lineWidth=2),xlabel('time (s)'),ylabel('q_x')
plot(t,q(:,3),lineWidth=2),xlabel('time (s)'),ylabel('q_y')
plot(t,q(:,4),lineWidth=2),xlabel('time (s)'),ylabel('q_z')
3.7 Exponential coordinates in SE(3)
3.7.1 Definition
Recall that the exponential map is used to map exponential coordinates to rotation matrices using
Inversely, rotation matrices can be mapped to exponential coordinates in SO(3) using the (matrix) logarithmic mapping,
Analogous to how exponential coordinates of rotations can be mapped to/from rotation matrices (or quaternions), there exist exponential coordinates of pose (position and orientation) that can be mapped to transformation matrices. The exponential coordinates in SE(3) are defined as
where
represents the rotational exponential coordinates, based on Section 3.4, and
describes the translational exponential coordinates, which is the displacement experienced within the vector space. The vector above is often also referred to as the twist vector. Importantly, this information will have different geometric meanings depending on in which frame it is described in. The tangent space representation of the exponential coordinates in SE(3) is given by
which can be thought of as the encoded matrix form of the exponential coordinates. Finally, the transformation matrix describing the pose is obtained by mapping the tangent space representation through the exponential mapping:
3.7.2 Left Jacobian of SO(3)
To avoid having to mathematically evaluate the matrix exponential every time we need to evaluate the equation above, we can instead make use of a compact closed form solution. Recall that the exponential mapping in SO(3) is given by
which provides us a direct means of determining the rotation matrix if we know the rotational exponential coordinates. Analogous to this, the transformation matrix can be determined by
where
is known as the left Jacobian of SO(3) and is given by and
Importantly, matrix
accounts for the fact that the robot is rotating while it is translating, creating a curved path rather than a straight line. The combination of Rodrigues' formula and the left Jacobian of SO(3) means that the transformation matrix can be easily determined if one knows the twist vector. 3.7.3 Body twist
In order to make use of the exponential coordinates in SE(3) in a meaningful way, we need to be aware of which reference frame is being used to represent the information. In this course we will predominantly base exponential coordinates in SE(3) in
. As such, we can represent the body-frame 6-vector exponential coordinates using Physically, we can interpret the vector above as describing a rigid body that has simultaneously experienced a rotation of α units about vector
and a displacement of the body-frame origin of
units. The usefulness of this formulation is that if the robot has some initial pose represented by
, and then subsequently experiences incremental motion as described in the equation above, we can then (exactly) update the pose representation of our robot using Note that this approach is analogous to how we incrementally updated the rotation matrix as shown in Section 3.6.1. Kinematically, the exponential coordinates are derived by obtaining the body-frame velocity information and then integrating the (assumed) constant velocity over the particular time frame. The 6-vector describing the body-frame velocity is commonly known as the body twist and is given by
where
is the body-frame angular velocity of the rigid body, and
is the body-frame translational velocity. The body twist can then by related to the exponential coordinates in SE(3) using where
is the duration of time in which the body twist was experienced. Example: Transformation matrix from body twist
Using MATLAB, determine the transformation matrix representing the pose of
with respect to
when a rigid body initial aligned with
experiences a translational velocity of
and a rotational velocity of
over a one second period. v = [vx vy vz]'; %body translational velocity vector
w = [wx wy wz]'; %body rotational velocity vector
twist = [w; v]; %body twist
expCoords = twist*dT; %exponential coordinates
S = [0 -expCoords(3) expCoords(2) expCoords(4); %tangent space representation of exponential coordinates
expCoords(3) 0 -expCoords(1) expCoords(5);
-expCoords(2) expCoords(1) 0 expCoords(6)
T = expm(S)
1.0000 0 0 0
0 0.5403 0.8415 0
0 -0.8415 0.5403 0
0 0 0 1.0000
plotTransforms([0 0 0],[1 0 0 0],"MeshFilePath",'multirotor.stl',"MeshColor","red")
axis equal,title('Before rotation and translation')
plotTransforms(tform2trvec(T),tform2quat(T),"MeshFilePath",'multirotor.stl',"MeshColor","blue")
axis equal,title('After rotation and translation')
sgtitle('Visualising a rotated and translated reference frame using exponential coordinates')